{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "Speed_PID_P=0.6\n",
    "Speed_PID_I=0.01\n",
    "\n",
    "CAR_WHEEL_WIDTH=0.19    # car wheel width (left and right)\n",
    "CAR_WHEEL_DISTANCE=0.13 # car front to back distance\n",
    "# 整车移动量转换为单轮Linear速度  x:前+后-  y:左+右-  z:逆+顺-\n",
    "def move_transform(vx, vy, vz):\n",
    "    '''\n",
    "    将整车的移动量转换为四个轮子的目标速度,单位为m/s\n",
    "\n",
    "    Args:\n",
    "        vx: 前后速度\n",
    "        vy: 左右速度\n",
    "        vz: 方向速度\n",
    "\n",
    "    Returns:\n",
    "        target_speed: 四个轮子的目标速度\n",
    "    '''\n",
    "    \n",
    "    a = vx - vy - vz * (CAR_WHEEL_DISTANCE / 2 + CAR_WHEEL_WIDTH / 2)\n",
    "    b = vx + vy + vz * (CAR_WHEEL_DISTANCE / 2 + CAR_WHEEL_WIDTH / 2)\n",
    "    c = vx + vy - vz * (CAR_WHEEL_DISTANCE / 2 + CAR_WHEEL_WIDTH / 2)\n",
    "    d = vx - vy + vz * (CAR_WHEEL_DISTANCE / 2 + CAR_WHEEL_WIDTH / 2)\n",
    "    # 计算每个轮子的目标速度\n",
    "    target_speed = [a, -1*b, c, -1*d]\n",
    "    return target_speed\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-16.0, -16.0, -16.0, -16.0]"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "move_transform(0,0,100)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-100.0, 100.0, -100.0, 100.0]"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "move_transform(-100,0,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def inverse_move_transform(wheel_speeds):\n",
    "    '''\n",
    "    从四个轮子的速度反推出整车的移动速度,单位为m/s\n",
    " \n",
    "    Args:\n",
    "        wheel_speeds: 四个轮子的速度列表，顺序为 [左前轮, 右前轮, 左后轮, 右后轮]\n",
    " \n",
    "    Returns:\n",
    "        vx: 前后速度\n",
    "        vy: 左右速度\n",
    "        vz: 方向速度\n",
    "    '''\n",
    "    output=[0,0,0,] # [vx,vy,vz]\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "CAR_WHEEL_WIDTH=0.19    # car wheel width (left and right)\n",
    "CAR_WHEEL_DISTANCE=0.13 # car front to back distance\n",
    "\n",
    "APB=(CAR_WHEEL_DISTANCE / 2 + CAR_WHEEL_WIDTH / 2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "maxtrix = np.array([[-1,1,APB],[1,1,-APB],[-1,1,-APB],[1,1,APB]])\n",
    "vector0 = np.array([0,0,0,0])\n",
    "vector1 = np.array([0,0,0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "def kn(vx,vy,vth):\n",
    "    speed=[0.0,0.0,0.0,0.0]\n",
    "    speed[1]=vx+vy-vth*APB\n",
    "    speed[0]=-vx+vy+vth*APB\n",
    "    speed[2]=-vx+vy-vth*APB\n",
    "    speed[3]=vx+vy+vth*APB\n",
    "    return speed"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "def kn_inv(spd):\n",
    "    speed=spd\n",
    "    vx = (speed[1] - speed[0] - speed[2] + speed[3]) / 4\n",
    "    vy = (speed[1] + speed[0]) / 2\n",
    "    vth = -(speed[1] - speed[0] + speed[2] - speed[3]) / (4 * APB)\n",
    "    return vx, vy, vth"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(10.0, 0.0, -0.0)"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "kn_inv(kn(10,0,0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-10.0, 10.0, -10.0, 10.0]"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "kn(10,0,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(0.0, 0.0, 62.5)"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "kn_inv([10.0, -10.0, -10.0, 10.0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(10.0, 0.0, -0.0)"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "kn_inv([-10.0, 10.0, -10.0, 10.0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
